# include "interpcpp.hpp"

# include <cassert>
# include <cstddef>
# include <iostream>
# include <math.h>
# include <utility>

# include "config.hpp"


namespace {

    // shortcut for data container
    typedef std::vector<NumType> array_type;

    typedef std::pair<size_t, size_t> int_pair;

    //
    // implementation of binary search algorithm
    //
    int_pair find_location(array_type const& v, NumType m) noexcept {
        assert(v.size() > 2);

        if (m < v.front())
            return std::make_pair(static_cast<size_t>(-1), 0);
        else if (m > v.back())
            return std::make_pair(v.size()-1, static_cast<size_t>(-1));

        size_t i0 = 0, i1 = v.size() - 1;
        while (i1 - i0 > 1) {
            size_t c = (i0 + i1) / 2;
            if (v[c] > m)
                i1 = c;
            else if (v[c] < m)
                i0 = c;
            else
                i0 = i1 = c;
        }

        return std::make_pair(i0, i1);
    }

// решение системы с трехдиагональной матрицей
// методом прогонки
// функция включается в библиотеку по причине
// отсутствия модуля по работе с линейной алгеброй
// аргументы функции
//   A  матрица с коэффициентами линейной системы
//   b  вектор свободных членов
//   N  размерность системы
// формат передачи данных:
//   b обычный массив длиной N
//   A обычный массив длиной 3N, первые N
//     элементов представляют собой поддиагональные
//     элементы
void
solve3d(NumType *A, NumType *b, unsigned int N) {
    register int i = 0;
    // прямой ход алгоритма
    for (i = 0; i < N-1; ++i) {
        b[i] = b[i]/A[N+i];
        A[2*N+i] = A[2*N+i]/A[N+i];
        b[i+1] = b[i+1]-b[i]*A[i+1];
        A[N+i+1] = A[N+i+1]-A[2*N+i]*A[i+1];
    };
    // вычисление последнего элемента
    b[N-1] = b[N-1]/A[2*N-1];
    // обратный ход прогонки
    for (i = N-2; i >= 0; --i)
        b[i] = b[i]-A[2*N+i]*b[i+1];
};


inline NumType FM(NumType const x, NumType const z0, NumType const z1) noexcept {
    double const dx = x - z0;
    double const dz = z1 - z0;

    return dx * dx * (3.0 * z1 - z0 - 2.0 * x) / dz / dz /dz;
};


inline NumType FP(NumType const x, NumType const z1, NumType const z2) noexcept {
    double const dx = x - z2;
    double const dz = z1 - z2;

    return dx * dx * (3.0 * z1 - z2 - 2.0 * x) / dz / dz / dz;
};


inline NumType GM(NumType const x, NumType const z0, NumType const z1) noexcept {
    double const dx = x - z0;
    double const dz = z1 - z0;

    return dx * dx * (x - z1) / dz / dz;
};


inline NumType GP(NumType const x, NumType const z1, NumType const z2) noexcept {
    double const dx = x - z2;
    double const dz = z1 - z2;

    return dx * dx * (x - z1) / dz / dz;
};

}


interpolation::LinearInterpolator::LinearInterpolator(array_type const& xs, array_type const& ys)
    : nodes_(new std::vector<NumType>(xs)), values_(ys)
{
    assert(xs.size() == ys.size());
    assert(xs.size() > 0);
}


NumType interpolation::LinearInterpolator::operator() (NumType x) const {
    std::vector<NumType> const& n = *nodes_.get();
    auto p = find_location(n, x);
    if (p.first == static_cast<size_t>(-1))
        return values_.front();

    else if (p.second == static_cast<size_t>(-1))
        return values_.back();

    else if (p.first == p.second)
        return values_[p.first];

    else {
        NumType k = (values_[p.second] - values_[p.first]) / (n[p.second] - n[p.first]);
        return values_[p.first] + k * (x - n[p.first]);
    }
}


interpolation::polynom::polynom(array_type const& xs, array_type const& ys)
    : nodes_(xs.size()), differences_(ys.size())
{
    assert(xs.size() == ys.size());
    assert(xs.size() > 1);

    for (size_t i = 0; i < xs.size(); ++i) {
        double const x = xs[i];
        nodes_[i] = x;
        differences_[i] = ys[i];

        for (size_t j = i - 1; j < xs.size(); --j) {
            double const dx = nodes_[j] - x;
            double const dy = differences_[j] - differences_[j + 1];
            differences_[j] = dy / dx;
        }

    }
}

NumType interpolation::polynom::operator() (NumType x) const {
    NumType summ = 0, prod = 1;
    for (size_t i = nodes_.size() - 1; i < nodes_.size(); --i) {
        summ += differences_[i] * prod;
        prod *= (x - nodes_[i]);
    }
    return summ;
}


interpolation::cubic_spline::cubic_spline(array_type const& xs, array_type const& ys)
    : nodes_(xs), values_(ys), ms_(xs.size(), 0)
{
    assert(xs.size() == ys.size());
    assert(xs.size() > 3);

    size_t const num_nodes = xs.size();

    NumType h1  = xs[1] - xs[0], h2 = 0;
    NumType df1 = ys[1] - ys[0], df2 = 0;
    std::vector<NumType> C(3 * num_nodes - 6);

    for (size_t i = 1; i < xs.size() - 1; ++i) {
        h2  = xs[i + 1] - xs[i];
        df2 = ys[i + 1] - ys[i];
        ms_[i] = df2 / h2 - df1 / h1;

        C[i-1] = h1 / 6.0;
        C[num_nodes + i - 3] = (h1 + h2) / 3.0;
        C[2 * num_nodes + i - 5] = h2 / 6.0;

        h1 = h2;
        df1 = df2;
    }
    C[3 * num_nodes - 1] = 0;

    solve3d(C.data(), &(ms_[1]), num_nodes - 2);
}


NumType interpolation::cubic_spline::operator() (NumType x) const {
    auto segment = find_location(nodes_, x);
    size_t p0 = segment.first, p1 = segment.second;
    if (p0 == static_cast<size_t>(-1)) {
        p0 = 0;
        p1 = 1;

    } else if (p1 == static_cast<size_t>(-1)) {
        p0 = nodes_.size() - 2;
        p1 = nodes_.size() - 1;

    } else if (p1 == p0)
        return values_[p0];

    NumType const d1 = nodes_[p1] - x;
    NumType const d0 = x - nodes_[p0];
    NumType const h  = nodes_[p1] - nodes_[p0];

    return ms_[p0] * d1 * d1 * d1 / 6.0 / h +
           ms_[p1] * d0 * d0 * d0 / 6.0 /h +
          (values_[p0] - ms_[p0] * h * h /6.0) * d1 / h +
          (values_[p1] - ms_[p1] * h * h /6.0) * d0 / h;
}


interpolation::hermite_spline::hermite_spline(array_type const& xs, array_type const& ys)
    : nodes_(xs), values_(ys), derivatives_(xs.size(), 0)
{
    assert(xs.size() == ys.size());
    assert(xs.size() > 0);

    // TODO :: replace approximation by 2-nd order formulas.
    // Approximate derivatives.
    for (size_t i = 0; i < xs.size() - 1; ++i) {
        double const dy = values_[i + 1] - values_[i - 1];
        double const dx = nodes_[i + 1] - nodes_[i - 1];
        derivatives_[i] = dy / dx;
    }

    derivatives_[0] = (values_[1] - values_[0]) / (nodes_[1] - nodes_[0]);
    size_t const j = nodes_.size() - 1;
    derivatives_[j] = (values_[j] - values_[j-1]) / (nodes_[j] - nodes_[j - 1]);
}


NumType interpolation::hermite_spline::operator() (NumType x) const {
    auto segment = find_location(nodes_, x);

    size_t p0 = segment.first, p1 = segment.second;
    if (p0 == static_cast<size_t>(-1)) {
        p0 = 0;
        p1 = 1;

    } else if (p1 == static_cast<size_t>(-1)) {
        p1 = nodes_.size() - 1;
        p0 = p1 - 1;

    } else if (p0 == p1)
        return values_[p0];

    return values_[p0] * FP(x, nodes_[p0], nodes_[p1]) +
           values_[p1] * FM(x, nodes_[p0], nodes_[p1]) +
           derivatives_[p0] * GP(x, nodes_[p0], nodes_[p1]) +
           derivatives_[p1] * GM(x, nodes_[p0], nodes_[p1]);
}


namespace {

    inline void do_search(std::vector<double> const& v, double x) {
        auto p = find_location(v, x);
        std::cout << "value " << x << " segment " << p.first << " :: " << p.second << std::endl;
    }

    void test_binary_search() {
        std::vector<double> u = {1, 2, 3, 4};
        do_search(u, 0.5);
        do_search(u, 1.5);
        do_search(u, 2.3);
        do_search(u, 45);
        do_search(u, 3);
    }

    void test_linear_interpolator() {
        using interpolation::LinearInterpolator;

        std::vector<double> xs, ys;
        for (int i = 0; i < 5; ++i) {
            xs.push_back(i);
            ys.push_back(i * i);
        }

        LinearInterpolator L = LinearInterpolator(xs, ys);

        double x = -1, dx = 0.05;
        while (x < 5.5) {
            std::cout << x << " " << L(x) << std::endl;
            x += dx;
        }
    }


    void test_polynom() {
        using interpolation::polynom;

        std::vector<double> xs, ys;
        for (int i = 0; i < 3; ++i) {
            xs.push_back(i);
            ys.push_back(i * i * i);
        }

        polynom p = polynom(xs, ys);
        double x = -1, dx = 0.05;
        while (x < 5.5) {
            std::cout << x << " " << p(x) << std::endl;
            x += dx;
        }
    }


    template <typename T>
    void test_spline() {
      std::vector<double> xs, ys;
      int N = 30;
      for (int i = 0; i < N; ++i) {
          double const x = 2 * M_PI * (i / (double) N);
          xs.push_back(x);
          ys.push_back(sin(x) + 2/(1.7 + cos(x)));
      }

      T p = T(xs, ys);
      double x = 0, dx = 0.05;
      while (x < 2 * M_PI) {
          std::cout << x << " " << p(x) << std::endl;
          x += dx;
      }
    }


    template <double (*f)(double, double, double)>
    void test_basis_function() {
        double x = 0.0, xe = 1.0, dx = 0.01;
        while (x < xe) {
            std::cout << x << " " << f(x, 0, 1) << std::endl;
            x += dx;
        }
    }

}


int main() {

//    test_basis_function<GM>();
    test_spline<interpolation::hermite_spline>();
//  test_spline<interpolation::cubic_spline>();
}
